(* # ===================================================================
   # Matrix Project
   # Copyright FEM-NUAA.CN 2020
   # =================================================================== *)


Require Import Reals.
Open Scope R_scope.
Require Export Matrix.Mat.RMatrix.
Require Export Matrix.Mat.RMtacs.

(** Ss -> Sg *)

(* 侧滑角β (sidelip angle) *)
Parameter beta : R.

(* φ   *)
Parameter phi   : R.

(* μ   *)
Parameter my    : R.

(* γ  *)
Parameter gamma : R.

Definition coordinate_transform_SaSg : Mat R 3 3 := mkMat_3_3
  ((cos my)*(cos phi))  
  ((sin my)*(cos phi)*(sin gamma)-(sin phi)*(cos gamma))
  ((sin my)*(cos phi)*(cos gamma)+(sin phi)*(sin gamma))
  ((cos my)*(sin phi))
  ((sin my)*(sin phi)*(sin gamma)+(cos phi)*(cos gamma))
  ((sin my)*(sin phi)*(cos gamma)-(cos phi)*(sin gamma))
  (-sin my) ((cos my)*(sin gamma)) ((cos my)*(cos gamma)).


(* 由  稳定坐标轴系(Ss)  转动侧滑角β到 气流坐标轴系(Sa) *)
Definition coordinate_transform_SsSa :  Mat R 3 3 := mkMat_3_3
  (cos beta)   (sin beta)   0
  (-sin beta)  (cos beta)   0
      0           0         1.

Definition SsToSg : Mat R 3 3 := mkMat_3_3
((cos beta)*(cos my)*(cos phi)-(sin beta)*((sin my)*(cos phi)*(sin gamma)-(sin phi)*(cos gamma)))
((sin beta)*(cos my)*(cos phi)+(cos beta)*((sin my)*(cos phi)*(sin gamma)-(sin phi)*(cos gamma)))
((sin my)*(cos phi)*(cos gamma)+(sin phi)*(sin gamma))
((cos beta)*(cos my)*(sin phi)-(sin beta)*((sin my)*(sin phi)*(sin gamma)+(cos phi)*(cos gamma)))
((sin beta)*(cos my)*(sin phi)+(cos beta)*((sin my)*(sin phi)*(sin gamma)+(cos phi)*(cos gamma)))
((sin my)*(sin phi)*(cos gamma)-(cos phi)*(sin gamma))
(-(cos beta)*(sin my)-(sin beta)*(cos my)*(sin gamma))
(-(sin beta)*(sin my)+(cos beta)*(cos my)*(sin gamma))
((cos my)*(cos gamma)).



Lemma SsToSg_eq :
  SsToSg === RMmul coordinate_transform_SaSg coordinate_transform_SsSa.
Proof.
  unfold SsToSg.
  RMat_mul_simpl. unfold mkMat_3_3'.
  f_equal2. ring. f_equal2. ring. ring. f_equal. ring.
  f_equal2. ring. ring. f_equal2. ring. f_equal. ring.
Qed. 
